function dxdt = ode_dt(t,x)

% 坐标系北天东
%{
[vx;
 vy;
 vz;
-((Fxb + Fxd)*(2*q_2^2 + 2*q_3^2 - 1) + (2*q_0*q_3 - 2*q_1*q_2)*(Fyb + Fyd) - (2*q_0*q_2 + 2*q_1*q_3)*(Fzb + Fzd))/m;
- g - ((Fyb + Fyd)*(2*q_1^2 + 2*q_3^2 - 1) - (2*q_0*q_3 + 2*q_1*q_2)*(Fxb + Fxd) + (2*q_0*q_1 - 2*q_2*q_3)*(Fzb + Fzd))/m;
-((Fzb + Fzd)*(2*q_1^2 + 2*q_2^2 - 1) + (2*q_0*q_2 - 2*q_1*q_3)*(Fxb + Fxd) - (2*q_0*q_1 + 2*q_2*q_3)*(Fyb + Fyd))/m;
- (omega_x*q_1)/2 - (omega_y*q_2)/2 - (omega_z*q_3)/2;
(omega_x*q_0)/2 - (omega_y*q_3)/2 + (omega_z*q_2)/2;
(omega_y*q_0)/2 + (omega_x*q_3)/2 - (omega_z*q_1)/2;
(omega_y*q_1)/2 - (omega_x*q_2)/2 + (omega_z*q_0)/2;
-(- m*omega_y*omega_z*r_Gy^2 + m*omega_y*omega_z*r_Gz^2 + Fyb*r_TBz + Fyd*r_TDz - Fzb*r_TBy - Fzd*r_TDy)/(m*r_Gx^2);
(- m*omega_x*omega_z*r_Gx^2 + m*omega_x*omega_z*r_Gz^2 + Fxb*r_TBz + Fxd*r_TDz - Fzb*r_TBx - Fzd*r_TDx)/(m*r_Gy^2);
-(- m*omega_x*omega_y*r_Gx^2 + m*omega_x*omega_y*r_Gy^2 + Fxb*r_TBy + Fxd*r_TDy - Fyb*r_TBx - Fyd*r_TDx)/(m*r_Gz^2)]
%}
%dxdt = zeros(13,1);
% 目标位置
global Init_Data State_Watch r_t
% Init_Data 质量 m,惯性半径r_Gx,r_Gy,r_Gz,气动参数 Cx0,Cy0,Cz0,参考面积 S_ref,重力加速度 g_0,空气密度 rho
% 弹体气动力到质心距离 r_TBx,r_TBy,r_TBz,舵面气动力到质心距离 r_TDx,r_TDy,r_TDz
num = 1;
m = Init_Data(num);num=num+1;r_Gx=Init_Data(num);num=num+1;r_Gy=Init_Data(num);num=num+1;r_Gz=Init_Data(num);num=num+1;
Cx0=Init_Data(num);num=num+1;Cy0=Init_Data(num);num=num+1;Cy_a=Init_Data(num);num=num+1;Cz0=Init_Data(num);num=num+1;Cz_b=Init_Data(num);num=num+1;
S_ref=Init_Data(num);num=num+1;g_0=Init_Data(num);num=num+1;rho=Init_Data(num);num=num+1;
r_TBx=Init_Data(num);num=num+1;r_TBy=Init_Data(num);num=num+1;r_TBz=Init_Data(num);num=num+1;
r_TDx=Init_Data(num);num=num+1;r_TDy=Init_Data(num);num=num+1;r_TDz=Init_Data(num);num=num+1;
%Fxd = 0;Fyd = 0;Fzd = 0;
% 状态量 x = [x y z vx vy vz q0 q1 q2 q3 omega_x omega_y omega_z]
%g = [0;-g_0;0];
g = g_0;
rx = x(1);
ry = x(2);
rz = x(3);
vx = x(4);
vy = x(5);
vz = x(6);
v = sqrt(vx^2 + vy^2 + vz^2);
q_0 = x(7);
q_1 = x(8);
q_2 = x(9);
q_3 = x(10);
q = [q_0;q_1;q_2;q_3];
omega_x = x(11);
omega_y = x(12);
omega_z = x(13);


C_BI = [1-2*(q_2^2+q_3^2) , 2*(q_1*q_2+q_0*q_3) , 2*(q_1*q_3-q_0*q_2) ;
2*(q_1*q_2-q_0*q_3) , 1-2*(q_1^2+q_3^2) , 2*(q_2*q_3+q_0*q_1) ;
2*(q_1*q_3+q_0*q_2) , 2*(q_2*q_3-q_0*q_1) , 1-2*(q_1^2+q_2^2)];
vb = C_BI*[vx;vy;vz];
vbx = vb(1);vby = vb(2);vbz = vb(3);
%计算攻角和侧滑角 单位为度
%fprintf('x:%.2f,y:%.2f,z:%.2f\n',rx,ry,rz);
%fprintf('vx:%.2f,vy:%.2f,vz:%.2f\n',vx,vy,vz);
%fprintf('vbx:%.2f,vby:%.2f,vbz:%.2f\n',vbx,vby,vbz);
alpha = atan2d(-vby, vbx);  % 迎角 α (°)
beta  = asind(vbz / sqrt(vbx^2 + vby^2 + vbz^2)); % 侧滑角 β (°)
%fprintf('速度:%.2f,攻角:%.2f,侧滑角:%.2f\n',v, alpha,beta);
% 采用远程火箭常用的坐标系转换方式
angle = my_quat2angle(q);
%fprintf('2-3-1欧拉角: phi:%.2f,psi:%.2f,gamma:%.2f\n', angle);
% dp Dynamic Pressure
dp = 0.5*rho*v^2;
Cx = Cx0;Cy = Cy0 + Cy_a*max(min(alpha,15),-15);Cz = Cz0 + Cz_b*max(min(beta,15),-15);
Fxb = -Cx*dp*S_ref;
Fyb = Cy*dp*S_ref;
Fzb = -Cz*dp*S_ref;
%fprintf('在弹体系中弹体产生的气动力:Fbx:%.2f,Fby:%.2f,Fbz:%.2f\n',Fxb,Fyb,Fzb);

%视加速度
persistent a_w control1I control2I
if isempty(a_w) % 仅在第一次调用时初始化
    a_w = zeros(3,1);
end
% 判断控制周期，和rk4积分周期
persistent inter_num
if isempty(inter_num)
    inter_num = 0;
else 
    inter_num = inter_num + 1;
end
% 比例导引加经典三回路自动驾驶仪 目标点位置 [30,1,6]
persistent theta_s_last psi_s_last delta_z delta_y n_target omega_t n_target_i r_ref
if isempty(delta_z)
    delta_z = 0;
end
if isempty(delta_y)
    delta_y = 0;
end
if isempty(n_target)
    n_target = zeros(2,1);
end
if isempty(omega_t)
    omega_t = zeros(3,1);
end
if isempty(n_target_i)
    n_target_i = zeros(2,1);
end
if isempty(r_ref)
    r_ref = zeros(3,1);
end
%控制周期
if mod(inter_num,4) == 0
    if isempty(control1I)
        control1I = 0;
    end
    if isempty(control2I)
        control2I = 0;
    end
    % 计算视线角速度,实际上由相机微分获得,存疑,无法使用广义比例导引法,弧度
    x_ref = r_t(1) - rx;y_ref = r_t(2) - ry;z_ref = r_t(3) - rz;
    r_ref = [x_ref;y_ref;z_ref];
    r_ref = C_BI*r_ref;
%     theta_s = atan2(y_ref,sqrt(x_ref^2+z_ref^2));
%     psi_s = atan2(-z_ref,x_ref);
    theta_s = atan2(r_ref(2),sqrt(r_ref(1)^2+r_ref(3)^2));
    psi_s = atan2(-r_ref(3),r_ref(1));

    dt = 0.005;
    if ~isempty(theta_s_last) && ~isempty(psi_s_last)
        omega_t = [(psi_s-psi_s_last)/dt*sin(theta_s);(psi_s-psi_s_last)/dt*cos(theta_s);(theta_s-theta_s_last)/dt];
    else 
        omega_t = zeros(3,1);
    end
    % 视线角速度限幅
    omega_t = max(min(omega_t, deg2rad(300)), deg2rad(-300));
    %omega_t(1) = max(min(omega_t(1), 300), -300);omega_t(2) = max(min(omega_t(2), 300), -300);omega_t(3) = max(min(omega_t(3), 300), -300);  
    theta_s_last = theta_s;
    psi_s_last = psi_s;
    V = 10;
    Ny = 3*V;
    Nz = 3*V;
    % 两轴过载指令
    n_target_i=[Ny*omega_t(3);-Nz*omega_t(2)]./g_0;
    %n_target = [0.02;0.01];
    n_target = n_target_i;
    % 驾驶仪参数
    K_1o = 1;K_1a = 0.1905;omega_1I = 9.8174;K_1R = -0.0584;
    % 采用鸭翼,俯仰通道方向,从弹体尾部,向上为正,低头
    %control1I = control1I + omega_1I*(K_1a*(n_target(1)*K_1o - a_w(2)/g_0) - omega_z);
    %delta_z = K_1R * control1I;
    control1I = control1I + 0.1*(-2.0*(n_target(1)*K_1o - a_w(2)/g_0));
    %control1I = max(min(control1I,10),-10);
    delta_z = 50*control1I + 25.0*omega_z ; %20*(-2.0*(n_target(1)*K_1o - a_w(2)/g_0));

    %delta_z = 45.0*(-2.0*(n_target(1)*K_1o - a_w(2)/g_0) + omega_z);
    % 驾驶仪参数
    K_2o = 1;K_2a = -0.1905;omega_2I = 9.8174;K_2R = -0.0584;
    % 采用鸭翼,偏航通道方向,从弹体尾部,向左为正,负偏航角
    % control2I = control2I + omega_2I*(K_2a*(n_target(2)*K_2o - a_w(3)) - omega_y);
    % delta_y = K_2R * control2I;
    
    control2I = control2I + 0.1*(2.0*(n_target(2)*K_2o - a_w(3)/g_0));
    %control2I = max(min(control2I,5),-5);
    delta_y = 50*control2I + 25.0*omega_y ;%+ 20*(2.0*(n_target(2)*K_2o - a_w(3)/g_0));
    %delta_y = 30.0*(2.0*(n_target(2)*K_2o - a_w(3)/g_0) + omega_y);
    % 滚转通道先不控 滚转角速度或角度稳定
    %delta_x = 3.0*(2.0);
end
%%%%%% 计算气动力
%delta_z = max(min(delta_z,30),-30);delta_y = max(min(delta_y,30),-30);
%Cdx = 0;Cdy = 0 + 0.03*max(min(delta_z,30),-30);Cdz = 0 + 0.03*max(min(delta_y,30),-30); 
Cdx = 0;Cdy = 0 + 0.03*delta_z;Cdz = 0 + 0.03*delta_y; 
% 采用鸭翼
Fxd = -Cdx*dp*S_ref;
% +舵 -力
Fyd = -Cdy*dp*S_ref;
% +舵 +力
Fzd = Cdz*dp*S_ref;
%Fzd = 0;
%fprintf('俯仰通道舵偏(抬头):%.2f,偏航通道舵偏(正偏航):%.2f',delta_z,delta_y);
%fprintf('俯仰通道舵偏:%.2f,偏航通道舵偏:%.2f',delta_z,delta_y);
% 1.8s前无控
% if t>1.8
%     Fxd = 0;Fyd = 0;Fzd = 0;
% end

dxdt = [vx;
        vy;
        vz;
        -((Fxb + Fxd)*(2*q_2^2 + 2*q_3^2 - 1) + (2*q_0*q_3 - 2*q_1*q_2)*(Fyb + Fyd) - (2*q_0*q_2 + 2*q_1*q_3)*(Fzb + Fzd))/m;
        - g - ((Fyb + Fyd)*(2*q_1^2 + 2*q_3^2 - 1) - (2*q_0*q_3 + 2*q_1*q_2)*(Fxb + Fxd) + (2*q_0*q_1 - 2*q_2*q_3)*(Fzb + Fzd))/m;
        -((Fzb + Fzd)*(2*q_1^2 + 2*q_2^2 - 1) + (2*q_0*q_2 - 2*q_1*q_3)*(Fxb + Fxd) - (2*q_0*q_1 + 2*q_2*q_3)*(Fyb + Fyd))/m;
        - (omega_x*q_1)/2 - (omega_y*q_2)/2 - (omega_z*q_3)/2;
        (omega_x*q_0)/2 - (omega_y*q_3)/2 + (omega_z*q_2)/2;
        (omega_y*q_0)/2 + (omega_x*q_3)/2 - (omega_z*q_1)/2;
        (omega_y*q_1)/2 - (omega_x*q_2)/2 + (omega_z*q_0)/2;
        -(- m*omega_y*omega_z*r_Gy^2 + m*omega_y*omega_z*r_Gz^2 + Fyb*r_TBz + Fyd*r_TDz - Fzb*r_TBy - Fzd*r_TDy)/(m*r_Gx^2);
        (- m*omega_x*omega_z*r_Gx^2 + m*omega_x*omega_z*r_Gz^2 + Fxb*r_TBz + Fxd*r_TDz - Fzb*r_TBx - Fzd*r_TDx)/(m*r_Gy^2);
        -(- m*omega_x*omega_y*r_Gx^2 + m*omega_x*omega_y*r_Gy^2 + Fxb*r_TBy + Fxd*r_TDy - Fyb*r_TBx - Fyd*r_TDx)/(m*r_Gz^2)];
if mod(inter_num,4) == 0
a_w = C_BI*(dxdt(4:6,1)+[0;g;0]);
watch = [t;x;angle;alpha;beta;vbx;vby;vbz;omega_x;omega_y;omega_z;Fxb;Fyb;Fzb;a_w;delta_z;delta_y;norm(q,2);Fxd;Fyd;Fzd;n_target;omega_t;control1I;control2I;n_target_i;r_ref];
State_Watch = [State_Watch,watch];
end
%fprintf('awx:%.2f,awy:%.2f,awz:%.2f\n',a_w);

end